﻿using DotNetMatrix;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Server.Utis
{
    class MatrixUtils
    {
        public static GeneralMatrix getRotateMatrixX(double angleInRad)
        {
            GeneralMatrix mRotate = new GeneralMatrix(3, 3);
            mRotate.SetElement(0, 0, 1);
            mRotate.SetElement(0, 1, 0);
            mRotate.SetElement(0, 2, 0);

            mRotate.SetElement(1, 0, 0);
            mRotate.SetElement(1, 1, Math.Cos(angleInRad));
            mRotate.SetElement(1, 2, -Math.Sin(angleInRad));

            mRotate.SetElement(2, 0, 0);
            mRotate.SetElement(2, 1, Math.Sin(angleInRad));
            mRotate.SetElement(2, 2, Math.Cos(angleInRad));

            return mRotate;
        }

        public static GeneralMatrix getRotateMatrixY(double angleInRad)
        {
            GeneralMatrix mRotate = new GeneralMatrix(3, 3);
            mRotate.SetElement(0, 0, Math.Cos(angleInRad));
            mRotate.SetElement(0, 1, 0);
            mRotate.SetElement(0, 2, Math.Sin(angleInRad));

            mRotate.SetElement(1, 0, 0);
            mRotate.SetElement(1, 1, 1);
            mRotate.SetElement(1, 2, 0);

            mRotate.SetElement(2, 0, -Math.Sin(angleInRad));
            mRotate.SetElement(2, 1, 0);
            mRotate.SetElement(2, 2, Math.Cos(angleInRad));

            return mRotate;
        }

        public static GeneralMatrix getRotateMatrixZ(double angleInRad)
        {
            GeneralMatrix mRotate = new GeneralMatrix(3, 3);
            mRotate.SetElement(0, 0, Math.Cos(angleInRad));
            mRotate.SetElement(0, 1, -Math.Sin(angleInRad));
            mRotate.SetElement(0, 2, 0);

            mRotate.SetElement(1, 0, Math.Sin(angleInRad));
            mRotate.SetElement(1, 1, Math.Cos(angleInRad));
            mRotate.SetElement(1, 2, 0);

            mRotate.SetElement(2, 0, 0);
            mRotate.SetElement(2, 1, 0);
            mRotate.SetElement(2, 2, 1);

            return mRotate;
        }

        public static GeneralMatrix getRotateMatrix(double angleInRadX, double angleInRadY, double angleInRadZ)
        {
            GeneralMatrix mX = getRotateMatrixX(angleInRadX);
            GeneralMatrix mY = getRotateMatrixY(angleInRadY);
            GeneralMatrix mZ = getRotateMatrixZ(angleInRadZ);

            return mZ.Multiply(mY).Multiply(mX);
        }

        public static GeneralMatrix getReverseRotateMatrix(double angleInRadX, double angleInRadY, double angleInRadZ)
        {
            GeneralMatrix mX = getRotateMatrixX(-angleInRadX);
            GeneralMatrix mY = getRotateMatrixY(-angleInRadY);
            GeneralMatrix mZ = getRotateMatrixZ(-angleInRadZ);

            return mX.Multiply(mY).Multiply(mZ);
        }
    }
}
